
import os
from types import resolve_bases
from typing import OrderedDict
import gdal
from numpy.ma.core import power, reshape
import xmltodict
from xml.dom.minidom import parse
import pandas
import numpy as np
import json
from matplotlib import pyplot as plt
import datetime
import yaml
import math
from decimal import Decimal # 为了更好的精度
import numba 
from numba import jit
from SatelliteOrbit import ReconstructionSatelliteOrbit
from util import GetVectorNorm,FindInfomationFromJson
class Orthorectification(object):
    '''
    正射校正模块类。
    '''

    def __init__(self,configPath="./config.ymal") -> None:
        super().__init__()
        # 常量声明
        # 加载配置文件
        with open(configPath,'r',encoding='utf-8') as f:
            cont = f.read()
            self.config = yaml.load(cont)
        self.config['SatelliteOribit']['StartTime']=datetime.datetime.strptime(self.config['SatelliteOribit']['StartTime']['Value'],self.config['SatelliteOribit']['StartTime']['format']).timestamp()
        self.R_e=self.config['SatelliteOribit']['ReferenceSpheroid']['Re'] # m
        self.R_p=self.config['SatelliteOribit']['ReferenceSpheroid']['Rp'] # m
        self.w_e=self.config['SatelliteOribit']['ReferenceSpheroid']['we'] # rad/s        

    def ParseHearderFile(self,HeaderFilePath_str,GPSNodeNames_list=['TimeStamp', 'xPosition', 'yPosition', 'zPosition', 'xVelocity', 'yVelocity', 'zVelocity']):
        '''
        解析头文件，返回计算所需要的必要信息:
        args:
            HeaderFilePath_str: 头文件地址，
        return:
            HeaderFileInfomation_json:头文件信息包
        '''

        with open(HeaderFilePath_str,'r',encoding='utf-8') as fp:
            HeaderFile_dom_str=fp.read()
        HeaderFile_dom=xmltodict.parse(HeaderFile_dom_str)  
        HeaderFile_dom_json=json.loads(json.dumps(HeaderFile_dom))
        # 获取头文件
        HeaderInformation_json={}
        # 解析轨道节点，假定是GPS节点。
        HeaderInformation_json['GPS']=[] 
        # GPS 解析信息
        GPSNode_Path=self.config["GPS"]['GPSNode_Path']
        GPSNodeNames_list=self.config["GPS"]['NodeInfomation_Name']
        GPSTime_Format=self.config['GPS']['Time_format']
        GPSPoints=FindInfomationFromJson(HeaderFile_dom_json,GPSNode_Path)
        for GPSPoint in GPSPoints:
            GPSPoint=[
                    datetime.datetime.strptime(GPSPoint[GPSNodeNames_list[0]],GPSTime_Format).timestamp(), # TimeStamp
                    float(GPSPoint[GPSNodeNames_list[1]]), # Xp
                    float(GPSPoint[GPSNodeNames_list[2]]), # Yp
                    float(GPSPoint[GPSNodeNames_list[3]]), # Zp
                    float(GPSPoint[GPSNodeNames_list[4]]), # Vx
                    float(GPSPoint[GPSNodeNames_list[5]]), # Vy
                    float(GPSPoint[GPSNodeNames_list[6]])] # VZ
            HeaderInformation_json["GPS"].append(GPSPoint)
        # 提取成像时间信息
        HeaderInformation_json['ImageInformation']={}
        # 1、开始成像时间
        HeaderInformation_json['ImageInformation']['StartTime']=datetime.datetime.strptime(
                FindInfomationFromJson(HeaderFile_dom_json,self.config['imageinfo']['StartImageTime']['NodePath']),
                self.config['imageinfo']['StartImageTime']['Format']
        ).timestamp()
        # 2、影像结束成像时间
        HeaderInformation_json['ImageInformation']['EndTime']=datetime.datetime.strptime(
                FindInfomationFromJson(HeaderFile_dom_json,self.config['imageinfo']['EndImageTime']['NodePath']),
                self.config['imageinfo']['StartImageTime']['Format']
        ).timestamp()
        # 3、影像的宽高
        HeaderInformation_json['ImageInformation']['height']=int(FindInfomationFromJson(HeaderFile_dom_json,self.config['imageinfo']['ImageHeight']['NodePath']))
        HeaderInformation_json['ImageInformation']['width']=int(FindInfomationFromJson(HeaderFile_dom_json,self.config['imageinfo']['ImageWidth']['NodePath']))
        # 4、影像的近斜距
        HeaderInformation_json['ImageInformation']['NearRange']=float(FindInfomationFromJson(HeaderFile_dom_json,self.config['imageinfo']['NearRange']['NodePath']))
        # 5、入射角
        HeaderInformation_json['ImageInformation']['incidenceAngle']={
            'NearRange':float(FindInfomationFromJson(HeaderFile_dom_json,self.config['imageinfo']['incidenceAngle']['NearRange']['NodePath'])),
            'FarRange':float(FindInfomationFromJson(HeaderFile_dom_json,self.config['imageinfo']['incidenceAngle']['FarRange']['NodePath']))
        }
        # 6、成像时刻影像带宽
        HeaderInformation_json['ImageInformation']['bandWidth']=float(FindInfomationFromJson(HeaderFile_dom_json,self.config['sensor']['bandWidth']['NodePath']))
        # 7、多普勒质心常数
        DopplerCentroidCoefficients_list=FindInfomationFromJson(HeaderFile_dom_json,self.config['imageinfo']['DopplerCentroidCoefficients']['NodePath'])
        HeaderInformation_json['ImageInformation']['DopplerCentroidCoefficients']=[
            float(DopplerCentroidCoefficients_list[self.config['imageinfo']['DopplerCentroidCoefficients']['DopplerCentroidCoefficients_Name'][0]]),
            float(DopplerCentroidCoefficients_list[self.config['imageinfo']['DopplerCentroidCoefficients']['DopplerCentroidCoefficients_Name'][1]]),
            float(DopplerCentroidCoefficients_list[self.config['imageinfo']['DopplerCentroidCoefficients']['DopplerCentroidCoefficients_Name'][2]]),
            float(DopplerCentroidCoefficients_list[self.config['imageinfo']['DopplerCentroidCoefficients']['DopplerCentroidCoefficients_Name'][3]]),
            float(DopplerCentroidCoefficients_list[self.config['imageinfo']['DopplerCentroidCoefficients']['DopplerCentroidCoefficients_Name'][4]]),
        ]
        # 8、波长
        HeaderInformation_json['ImageInformation']['lambda']=float(FindInfomationFromJson(HeaderFile_dom_json,self.config['sensor']['lambda']['NodePath']))
        # 9、中心像元对应得中心经纬度
        CenterPoint=FindInfomationFromJson(HeaderFile_dom_json,self.config['imageinfo']['CenterImagePositon']['NodePath'])
        HeaderInformation_json['ImageInformation']['centerPosition']=[
            float(CenterPoint[self.config['imageinfo']['CenterImagePositon']['Value'][0]]), # 纬度
            float(CenterPoint[self.config['imageinfo']['CenterImagePositon']['Value'][1]]), # 经度
        ]
        # 10、多普勒参数参考时间
        HeaderInformation_json['ImageInformation']['DopplerParametersReferenceTime']=float(FindInfomationFromJson(HeaderFile_dom_json,self.config['imageinfo']['DopplerParametersReferenceTime']['NodePath']))
        # 11、参考斜距
        HeaderInformation_json['ImageInformation']['refRange']=float(FindInfomationFromJson(HeaderFile_dom_json,self.config['imageinfo']['ReferenceRange']['NodePath']))
        HeaderInformation_json['PRF']=float(FindInfomationFromJson(HeaderFile_dom_json,self.config['sensor']['PRF']['NodePath']))
        return HeaderInformation_json
        pass
   
    def LLA_to_XYZ(self,latitude, longitude, altitude):
        ''' 经纬度转大地坐标系
        args:
            latitude:纬度
            longitude:经纬
            altitude:海拔高程
        refrence: https://blog.csdn.net/dulingwen/article/details/96868530
        '''
        # 经纬度的余弦值
        cosLat = math.cos(latitude * math.pi / 180)
        sinLat = math.sin(latitude * math.pi / 180)
        cosLon = math.cos(longitude * math.pi / 180)
        sinLon = math.sin(longitude * math.pi / 180)
    
        # WGS84坐标系的参数
        rad = 6378137.0        #地球赤道平均半径（椭球长半轴：a）
        f = 1.0 / 298.257224   # WGS84椭球扁率 :f = (a-b)/a
        C = 1.0 / math.sqrt(cosLat * cosLat + (1-f) * (1-f) * sinLat * sinLat)
        S = (1-f) * (1-f) * C
        h = altitude
    
        # 计算XYZ坐标
        X = (rad * C + h) * cosLat * cosLon
        Y = (rad * C + h) * cosLat * sinLon
        Z = (rad * S + h) * sinLat
    
        return np.array([X, Y, Z]).reshape(1,3)
   
    def XYZ_to_LLA(self,X, Y, Z):
        ''' 大地坐标系转经纬度
        适用于WGS84坐标系
        args:
            x,y,z
        return:
            lat,long,altitude
        '''
        # WGS84坐标系的参数
        a = 6378137.0        # 椭球长半轴
        b = 6356752.314245   # 椭球短半轴
        ea = np.sqrt((a ** 2 - b ** 2) / a ** 2)
        eb = np.sqrt((a ** 2 - b ** 2) / b ** 2)
        p = np.sqrt(X ** 2 + Y ** 2)
        theta = np.arctan2(Z * a, p * b)
    
        # 计算经纬度及海拔
        longitude = np.arctan2(Y, X)
        latitude = np.arctan2(Z + eb ** 2 * b * np.sin(theta) ** 3, p - ea ** 2 * a * np.cos(theta) ** 3)
        N = a / np.sqrt(1 - ea ** 2 * np.sin(latitude) ** 2)
        altitude = p / np.cos(latitude) - N
    
        return np.array([np.degrees(latitude), np.degrees(longitude), altitude])

 
    def getRByColumncode(self,c):
        '''
        根据列号计算斜距
        args:
            c：列号
        '''
        return  self.R0+c*self.delta_R
        pass
 
    def getTimeByLineCode(self,r):
        '''
        根据行号计算成像时间
        args:
            r：行号
        '''
        return np.matmul(np.concatenate([ np.array([r**i]).reshape(1,1) for i in range(self.r2t_A_arr.shape[0])],axis=1),self.r2t_A_arr)


    def ReconstuctionTimesOflyDirectionPositionModel(self,timePoints_list):
        ''' 构建飞行向坐标与时间的转换模型,注意这里没有调整时间的起算点。
            args:
                timePoints_list:时间点坐标 [r,t]
        '''
        # 根据点数确定模型，最高为3次项模型
        Y=np.zeros((len(timePoints_list),1)) # 确定Y
        X=np.zeros((len(timePoints_list),1))
        for i in range(len(timePoints_list)):
            Y[i,0]=timePoints_list[i][1]
            X[i,0]=timePoints_list[i][0]
        if len(timePoints_list)==2:
            # 一次项
            X=np.concatenate([np.ones((len(timePoints_list),1)),X],axis=1)
            A=np.matmul(np.matmul(np.linalg.inv(np.matmul(X.T,X)),X.T),Y)

        elif len(timePoints_list)>=3: # 二次项
            X=np.concatenate([np.ones(len(timePoints_list),1),X,np.power(X,2)],axis=1)
            A=np.matmul(np.matmul(np.linalg.inv(np.matmul(X.T,X)),X.T),Y)
            pass
        elif len(timePoints_list)>=8: # 三次项
            X=np.concatenate([np.ones(len(timePoints_list),1),X,np.power(X,2),np.power(X,3)],axis=1)
            A=np.matmul(np.matmul(np.linalg.inv(np.matmul(X.T,X)),X.T),Y)
            pass
        self.r2t_A_arr=A

    def PrepareConverteSystem(self):
        ''' 计算常量
        在数据计算之前，提前处理部分常量信息
        1、构建r,c 到坐标斜距的计算公式
        '''
        R0=self.header_info['ImageInformation']['NearRange'] # 起始斜距，近斜距
        delta_R=self.config['LightSpeed']/(2*self.header_info['ImageInformation']['bandWidth']*10**6)# 计算距离向分辨率 ,参考《InSAR原理与应用》P36，距离向分辨率 带宽单位为MHZ
        self.ReconstuctionTimesOflyDirectionPositionModel([
            [0,self.header_info['ImageInformation']['StartTime']],
            [self.header_info['ImageInformation']['height']-1,self.header_info['ImageInformation']['EndTime']]
            ]) # 构建坐标到时间的变换
        self.R0=R0
        self.delta_R=delta_R

    
    
    def Orthorectification(self,FilePath_str):
        '''
        正射校正组件。
        正射校正整体可以分为两个步骤：
        Step 1：计算出栅格坐标对应的真实的大地坐标系坐标
        Step 2：根据大地坐标系将影像重采样到大地坐标系空间中。
        args:
            FilePath_str:影像所在文件夹
        '''
        # 分离需要校正对象
        file_name_list=os.listdir(FilePath_str) 
        header_name=[file_name for file_name in file_name_list if file_name.rfind(".meta.xml")==len(file_name)-len('.meta.xml')][0] # 头文件
        tiff_name=[file_name for file_name in file_name_list if file_name.rfind(".tiff")==len(file_name)-len('.tiff')] # 影像文件
        # 解析头文件
        self.header_info=self.ParseHearderFile(os.path.join(FilePath_str,header_name))
        # 构建轨道模型
        self.SatelliteOrbitModel=ReconstructionSatelliteOrbit(self.header_info['GPS'],starttime=self.config['SatelliteOribit']['StartTime']) # 构建卫星轨道模型,取第0个节点的时间
        # 求解模型前计算常数变换模型
        self.PrepareConverteSystem()
        # 测试第0，0个点的空间坐标
        self.ConverteCoordinaryPoint_single(0,0)
        # 批量计算空间坐标
        self.ConverteCoordinary()

   
    def ConverteCoordinary(self):
        '''
        批量求解点坐标
        '''
        pass
   
    def ConverteCoordinaryPoint_single(self,r,c,h=0,breakCondition=1):
        '''
        求解单个点坐标
        args:
            r: 行号 height
            c: 列号 width
            h：平均大地高
            breakCondition: 迭代终止条件，默认值为1
        '''
        pass


    
class Orthorectification_RD(Orthorectification):
    '''
    正射校正模块类。
    '''
    def __init__(Orthorectification_RD,configPath="./config.ymal") -> None:
        super().__init__(configPath)  

    def ConverteCoordinary(self):
        '''
        批量求解点坐标
        '''
        # 构建映射表 ----------------------------------需要重写代码
        result={}
        print("开始时间：",datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f"))
        alpha0=0
        beta0=0
        for r in range(self.header_info['ImageInformation']['height']):
            if r%10==0: 
                print("r={} 时间：".format(str(r)),datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f"))
            for c in range(self.header_info['ImageInformation']['width']):
                result[(r,c)]=self.ConverteCoordinaryPoint_single(r,c,alpha0=alpha0,beta0=beta0)
                alpha0=result[(r,c)][1]
                beta0=result[(r,c)][2]
        print("终止时间：",datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f"))
        pass

    def ConverteCoordinary(self,OutputPath=None,DEM=None,BlocksPath="./OrthorecticationGrid",BlockSize=1):
        '''批量转换坐标
        采用矩阵结构，批量转换点，并将坐标点保存到指定文件分块文件中，并最终汇总成一个大型文件。
        Args:
            OutputPath: 最终文件地址，如果为None，则不把分块文件进行汇总成一个整体文件。
            DEM：与坐标对应的高程，default:None
            BlocksPath:分块存储地址与最终集合地址
            BlockSize:块大小，default：1 GB
        return:
            Result_path: 最终文件地址
        '''
        # 数据参数预读取
        # 多普勒参数
        Dopper_d=np.array(self.header_info['ImageInformation']['DopplerCentroidCoefficients']).reshape(-1,1)
        LightSpeed=self.config['LightSpeed']
        # T0
        DopplerParametersReferenceTime=self.header_info['ImageInformation']['refRange']*2/LightSpeed
        # 波长
        lamda=self.header_info['ImageInformation']['lambda']
        # Re,Rp,we
        Re=self.config['SatelliteOribit']['ReferenceSpheroid']['Re']
        Rp=self.config['SatelliteOribit']['ReferenceSpheroid']['Rp']
        we=self.config['SatelliteOribit']['ReferenceSpheroid']['we'] # we 地球自转速度矢量 --- 废弃        
        # 影像宽高
        height=self.config['ImageInformation']['height']
        width=self.config['ImageInformation']['width']
    
        # 计算分块文件组件
        # 坐标计算按照行进行起算，
        size_max=BlockSize*1024*1024*1024 # 对应的字节
        
        pass
    
    def Sample_sinc(self):
        # 插值计算点坐标
        pass


    def ConverteCoordinaryPoint_single(self,r,c,h=0,breakCondition=1,alpha0=0,beta0=0):
        '''
        求解单个点坐标
        args:
            r: 行号 height
            c: 列号 width
            h：平均大地高
            breakCondition: 迭代终止条件，默认值为1
        return:
            TargetPosition_:点坐标
        版本改动： 
        2021.8.19  尝试将椭球方程进行变形 
        2021.8.22  方法重写
        '''
        # 计算对应成像时间和成像斜距
        R=self.getRByColumncode( c) # 斜距
        t=self.getTimeByLineCode( r) # 成像时间
        # 解析成像时刻，卫星空间位置
        SatelliteState=self.SatelliteOrbitModel.SatelliteSpaceState(t)  
        [Sate_time,Sate_x,Sate_y,Sate_z,Sate_vx,Sate_vy,Sate_vz]=SatelliteState.reshape(1,7).tolist()[0]  
        # 多普勒
        Dopper_d=np.array(self.header_info['ImageInformation']['DopplerCentroidCoefficients']).reshape(-1,1)
        LightSpeed=self.config['LightSpeed']
        # DopplerParametersReferenceTime=self.header_info['ImageInformation']['DopplerParametersReferenceTime']

        DopplerParametersReferenceTime=self.header_info['ImageInformation']['refRange']*2/LightSpeed
        # 波长
        lamda=self.header_info['ImageInformation']['lambda']
        # Re,Rp,we
        Re=self.config['SatelliteOribit']['ReferenceSpheroid']['Re']
        Rp=self.config['SatelliteOribit']['ReferenceSpheroid']['Rp']
        we=self.config['SatelliteOribit']['ReferenceSpheroid']['we'] # we 地球自转速度矢量 
        # 使用牛顿迭代法，求解Xp,Yp,Zp 
        # 椭球体方程常数 a+h ==>a  b==>b
        a=(Re+h)**2
        b=Rp**2
        # 初始迭代点为中心像元经纬度对应的大地坐标系
        #XYZ_=self.LLA_to_XYZ(self.header_info['ImageInformation']['centerPosition'][0],
                            #self.header_info['ImageInformation']['centerPosition'][1],
                            #h)
        XYZ=np.array([Sate_x,Sate_y,Sate_z]).reshape(1,3)
        # 确定初始值，作为求解起始值
        TargetPosition=math.sqrt( 1/((Sate_x**2+Sate_y**2)/a+(Sate_z**2)/b))
        TargetPosition=TargetPosition*np.array([Sate_x,Sate_y,Sate_z]).reshape(1,3)
        TargetPosition=TargetPosition.reshape(1,3)
        R_threshold= np.sqrt( np.sum( np.power(TargetPosition-XYZ,2) ))
        SatellitePosition=np.array([Sate_x,Sate_y,Sate_z]).reshape(3,1)
        SatelliteVelocity=np.array([Sate_vx,Sate_vy,Sate_vz]).reshape(3,1)
        TargetPosition_,alpha,beta=self.ASF(R,SatellitePosition,SatelliteVelocity,TargetPosition,Dopper_d,LightSpeed,DopplerParametersReferenceTime,lamda,R_threshold,H=h,alpha0=alpha0,beta0=beta0)
        ''' 
        # 中间检查有无错误
        TargetPositionLLA=self.XYZ_to_LLA(TargetPosition[0,0],TargetPosition[0,1],TargetPosition[0,2]).reshape(1,-1)
        TargetPosition_LLA=self.XYZ_to_LLA(TargetPosition_[0,0],TargetPosition_[0,1],TargetPosition_[0,2]).reshape(1,-1)
        np.set_printoptions(suppress=True)
        print(TargetPosition.reshape(1,-1))
        print(TargetPosition_.reshape(1,-1))
        print(TargetPositionLLA.reshape(1,-1))
        print(TargetPosition_LLA.reshape(1,-1))
        print(TargetPosition_-TargetPosition)
        print(TargetPosition_LLA-TargetPositionLLA)
        np.set_printoptions(suppress=False)
        '''
        return TargetPosition_.reshape(1,-1),alpha,beta


    
    '''
    RD模型解算模块,这里主要使用ASF方法
    reference: 陈尔学. 星载合成孔径雷达影像正射校正方法研究[D].中国林业科学研究院,2004. 第三章内容
                且假定地表运动速度为0
    '''
   
    def ASF(self,R,SatellitePosition,SatelliteVelocity,TargetPosition,Dopper_d,LightSpeed,DopplerParametersReferenceTime,lamda,R_threshold,H=0,alpha0=0,beta0=0):
        # 部分参数初始化
        alpha=alpha0
        beta=beta0
        delta_alpha=0


        Dopper_d=Dopper_d.reshape(-1,1)
        # 这里需要重构
        TSR=2*R/LightSpeed
        T0=DopplerParametersReferenceTime
        TSR=TSR-T0
        # 这个公式是根据《RADARSAT-2 PRODUCT FORMAT DEFINITION》(RadarSat2的数据格式手册) 
        # “table 5-17 Doppler Rate Values” 中 dopplerRateValuesCoefficients 一行
        # ist of up to 5 Doppler rate values coefficients as a function of slant range time: r0, r1, r2, r3, and r4 where: 
        # Doppler rate  values = r0 + r1(tSR - t0) + r2(tSR-t0)^2 + r3(tSR-t0)^3 + r4(tSR-t0)^4
        fd=np.matmul(np.array([TSR**0,TSR**1,TSR**2,TSR**3,TSR**4]).reshape(1,5),Dopper_d)   
        we=self.w_e
        M=self.SatelliteCoordination2ECR(SatellitePosition,SatelliteVelocity)
        i=0 # 统计迭代次数
        while True:
            TargetVelocity=np.array([0,0,0]).reshape(3,1)# np.array([-1*we*TargetPosition[1,0],we*TargetPosition[0,0],0]) # 计算地面速度,粗糙计算时，默认为0
            beta=self.GetLookFromRangeYaw(R,alpha,beta,SatellitePosition,SatelliteVelocity,R_threshold,H=H)
            FD_=self.FD(alpha,beta,SatelliteVelocity,TargetVelocity,R,lamda,M)
            delta_fd=FD_-fd
            delta_alpha=-1*delta_fd*lamda/(2* np.sum(np.sqrt(np.power(SatelliteVelocity-TargetVelocity,2))))
            if np.abs(delta_alpha*R)<0.1:
                XYZ=self.GetXYZByBetaAlpha(alpha,beta,SatellitePosition,R,M)
                break
            alpha=alpha+delta_alpha
            i=i+1
        return XYZ.reshape(1,-1),alpha,beta

   
    def GetLookFromRangeYaw(self,R,alpha,beta,SatellitePosition,SatelliteVelocity,R_threshold,H=0):
        '''
        根据R和alpha 计算出beta来。
        根据参考论文，式3-13，完成此方法。（注意这是近似法
        args:
            R：斜距
            alpha:雷达侧视角
            beta0:beta初始值
            SatellitePosition:3x1 卫星空间位置
            TargetPosition：3x1 目标对象
        '''
        M=self.SatelliteCoordination2ECR(SatellitePosition,SatelliteVelocity)
        # 地球半径
        Rp=self.R_p+H
        Rs_norm=math.sqrt(np.matmul(SatellitePosition.T,SatellitePosition).reshape(1,1)[0,0])
        # 初始化beta
        if beta==0:
            beta_cos=(Rs_norm**2+R**2-Rp**2)/(2*Rs_norm*R)
            beta=math.acos(beta_cos)
        # 迭代
        i=0
        while True:
            # 计算斜率的增加
            delta_R=R-self.FR(alpha,beta,SatellitePosition,M,R_threshold,R,H=H) 
            # 计算入射角
            sin_eta=Rs_norm*math.sin(beta)/Rp
            tan_eta=sin_eta/math.sqrt(1-sin_eta**2)
            # 计算增量
            delta_beta=delta_R/(R*tan_eta)
            # 更新
            beta=beta+delta_beta
            # 判断循环终止条件
            if np.abs(delta_R)<0.1:
                break
            i=i+1
            if i>=10000: # 达到终止条件
                return None
        return beta
   
    def XYZOuter(self,A,B):
        '''
        外积（叉乘）,日后版本换成可以任意维度的外积运算方程
        args:
          A:3x1
          B:3x1
        '''
        C=np.cross(A.reshape(1,3),B.reshape(1,3)).reshape(-1,1)
        return C

    def SatelliteCoordination2ECR(self,SatellitePosition,SatelliteVelocity):
        '''
        将卫星为中心的直角坐标系Sxyz转换为ECR坐标系。
        M=[x,y,z]
        args:
            SatellitePosition:np.array 3x1  ECR坐标系下，卫星空间位置
            SatelliteVelocity:np.array 3x1   ECR坐标系下，卫星运动速度
        return:
            坐标系变换矩阵 M
            ECR=M*Sxyz
            P‘=MP
        '''
        z=SatellitePosition/GetVectorNorm(SatellitePosition)

        y=self.XYZOuter(SatellitePosition,SatelliteVelocity)/(np.sqrt(np.matmul(SatellitePosition.T,SatellitePosition))*np.sqrt(np.matmul(SatelliteVelocity.T,SatelliteVelocity))) # 外积
        x=self.XYZOuter(y,z)
        M=np.concatenate([x.reshape(3,1),y.reshape(3,1),z.reshape(3,1)],axis=1) # 以列组合
        return M
   
    def UnitVectorOfSatelliteAndTarget(self,alpha,beta,M):
        '''
        获取从卫星指向地面目标的单位矢量
        '''
        # 目标T的单位向量 ST=R*M*P
        P=np.array([math.sin(alpha),-math.cos(alpha)*math.sin(beta),-math.cos(alpha)*math.cos(beta),]).reshape(3,1)
        P_=np.matmul(M,P).reshape(-1,1)
        return P_
   
    def FD(self,alpha,beta,SatelliteVelocity,TargetVelocity,R,lamda,M):
        '''
        根据beta,alpha,卫星空间位置,斜距，转换矩阵
        '''
        we=self.w_e
        #
        TargetVelocity=0 # 假设地面运动为0
        UnitVector=self.UnitVectorOfSatelliteAndTarget(alpha,beta,M)
        Rst=-1*R*UnitVector
        Rst_Vst=np.matmul(Rst.T,SatelliteVelocity-TargetVelocity)
        fd=-1*(2/lamda)*Rst_Vst/R
        return fd
   
    def GetXYZByBetaAlpha(self,alpha,beta,SatellitePosition,R,M):
        '''
        从beta，alpha获取获取目标的空间位置
        '''
        P=np.array([math.sin(alpha),-math.cos(alpha)*math.sin(beta),-math.cos(alpha)*math.cos(beta),]).reshape(3,1)
        Rt=SatellitePosition+R*np.matmul(M,P)
        return Rt
   
    def FR(self,alpha,beta,SatellitePosition,M,R_threshold,R_ref,H=0):
        '''
        根据 雷达侧视角，雷达视角，卫星位置，坐标系变换矩阵，大地高等参数，根据参考论文中公式3-1 ==》3-10的计算过程，设计此方程
        args:
            alpha:雷达侧视角 卫星与地物间速度运动差导致的（即多普勒频移的结果）
            beta: 雷达视角
            Satellite_position:卫星空间位置
            M：坐标系转换矩阵
            H：大地高
        return:
            R：斜距
        '''
       
        UnitVector=self.UnitVectorOfSatelliteAndTarget(alpha,beta,M)
        # 解方程 3-10 纠正错误，A公式有问题
        a=(self.R_e+H)**2
        b=self.R_p**2
        [Xs,Ys,Zs]=SatellitePosition.reshape(-1,1)
        [Xp,Yp,Zp]=UnitVector.reshape(-1,1)
        A=(Xp**2+Yp**2)/a+(Zp**2)/b
        B=2*(Xs*Xp+Ys*Yp)/a+2*Zs*Zp/b
        C=(Xs**2+Ys**2)/a+Zs**2/b
        C=C-1
        t=B**2-4*A*C
        #R=(-B+math.sqrt(t))/(2*A) # 这里通过几何分析排除这个解
        R1=(-B-math.sqrt(t))/(2*A)
        if R1>R_threshold:
            return R1
        return None

class Orthorectification_PSTN(Orthorectification):
    '''间接定位法
    '''
    def Orthorectification(self,FilePath_str):
        '''
        正射校正组件。
        正射校正整体可以分为两个步骤：
        Step 1：计算出栅格坐标对应的真实的大地坐标系坐标
        Step 2：根据大地坐标系将影像重采样到大地坐标系空间中。
        args:
            FilePath_str:影像所在文件夹
        '''
        # 分离需要校正对象
        file_name_list=os.listdir(FilePath_str) 
        header_name=[file_name for file_name in file_name_list if file_name.rfind(".meta.xml")==len(file_name)-len('.meta.xml')][0] # 头文件
        tiff_name=[file_name for file_name in file_name_list if file_name.rfind(".tiff")==len(file_name)-len('.tiff')] # 影像文件
        # 解析头文件
        self.header_info=self.ParseHearderFile(os.path.join(FilePath_str,header_name))
        # 构建轨道模型
        self.SatelliteOrbitModel=ReconstructionSatelliteOrbit(self.header_info['GPS'],starttime=self.config['SatelliteOribit']['StartTime']) # 构建卫星轨道模型,取第0个节点的时间
        # 求解模型前计算常数变换模型
        self.PrepareConverteSystem()
        # 测试第0，0个点的空间坐标
        Doppler_d=np.array(self.header_info['ImageInformation']['DopplerCentroidCoefficients']).reshape(-1,1)
        LightSpeed=self.config['LightSpeed']
        T0=self.header_info['ImageInformation']['refRange']*2/LightSpeed
        # -2312340.457	5393726.182	2489909.349
        TargetPosition=np.array([-2312340.457,	5393726.182	,2489909.349]).reshape(1,-1)
        #TargetPosition=np.array([-2366713.39445398 , 5350367.51812531 , 2531732.36706542]).reshape(1,-1)
        lamda=self.header_info['ImageInformation']['lambda']
        StartTime=self.header_info['ImageInformation']['StartTime']
        PRF=self.header_info['PRF']
        self.PSTN(TargetPosition,Doppler_d,StartTime,lamda,T0,LightSpeed,PRF)
   
    def ConverteCoordinary(self):
        '''
        批量求解点坐标
        '''
        pass
   
    def ConverteCoordinaryPoint_single(self,r,c,h=0,breakCondition=1):
        '''
        求解单个点坐标
        args:
            r: 行号 height
            c: 列号 width
            h：平均大地高
            breakCondition: 迭代终止条件，默认值为1
        '''
        pass    
    def PSTN(self,TargetPostion,Doppler_d,StartTime,lamda,T0,LightSpeed,PRF):
        '''间接定位法
        Args:
            TargetPostion:地面坐标
            Doppler_d:多普勒系数
            StartTime:成像开始时间
            lamda: 波长
            T0: 多普勒参考时间
            LightSpeed: 光速
        return:
            r,c:影像的行列号
        '''
        ti=StartTime
        Doppler_d=Doppler_d.reshape(-1,1)
        dt=0.001
        delta_t=1/PRF
        delta_R=self.delta_R
        i=0
        while True:
            # 卫星轨道
            statellitePosition=self.SatelliteOrbitModel.SatelliteSpaceState(ti) # 卫星轨道
            statellite_XYZ=statellitePosition[0,1:4]
            statellite_velocity=statellitePosition[0,4:]
            # 位置向量
            R_sp=statellite_XYZ-TargetPostion.reshape(1,-1)
            V_sp=statellite_velocity
            inc_r=GetVectorNorm(V_sp)
            # 斜距
            R=GetVectorNorm(R_sp)
            # 计算多普勒频率
            fde=-(2/lamda)*np.matmul(R_sp.reshape(1,3),V_sp.reshape(3,1))/R  # 理论
            TSR=R*2/LightSpeed-T0
            fd=np.matmul(np.array([TSR**0,TSR**1,TSR**2,TSR**3,TSR**4]).reshape(1,5),Doppler_d) #t=ti
            # 计算多普勒频率的导数
            t=ti+dt
            satellitionPostion_t=self.SatelliteOrbitModel.SatelliteSpaceState(t)
            sate_position=satellitionPostion_t[0,1:4]
            sate_velocity=satellitionPostion_t[0,4:]
            R_sp_t=sate_position-TargetPostion
            V_sp_t=sate_velocity
            R_t=GetVectorNorm(R_sp_t)
            fd_dt=-(2/(lamda*R_t))*np.matmul(R_sp_t.reshape(1,3),V_sp_t.reshape(3,1))
            fd_grad=(fd_dt-fde)/dt
            inc_t=(fde-fd)/fd_grad # 求得变量
            if np.abs(inc_t)<delta_t*0.001:
                break
            ti=ti-inc_t
            i=i+1
        # 计算行号
        r=(ti-StartTime)/delta_t
        c=(R-self.header_info['ImageInformation']['NearRange'])/delta_R
        # 取整
        r=np.round(r)
        c=np.round(c)
        return r,c




# 数据测试模块
if __name__=="__main__":
    tiff_path="./data/4-GF3_KAS_FSII_020008_E113.2_N23.1_20200528_L1A_HHHV_L10004829485/4-GF3_KAS_FSII_020008_E113.2_N23.1_20200528_L1A_HHHV_L10004829485"
    Orthorectification_PSTN_object=Orthorectification_PSTN()
    Orthorectification_PSTN_object.Orthorectification(tiff_path)
    #Orthorectification_RD_object=Orthorectification_RD()
    #Orthorectification_RD_object.Orthorectification(tiff_path)
    pass



